/*
 * Copyright (C) 2022 HiHope Open Source Organization .
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 *
 * limitations under the License.
 */

#include <stdio.h>
#include <stdlib.h>
#include <memory.h>
#include <unistd.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "iot_gpio_ex.h"
#include "iot_errno.h"
#include "iot_uart.h"
#include "iot_watchdog.h"

//网络头文件-----------start-------------
#include "wifi_device.h"
#include "lwip/netifapi.h"
#include "lwip/api_shell.h"
#include <netdb.h>
#include <string.h>
#include <stdlib.h>
#include "lwip/sockets.h"
#include "wifi_connect.h"
//网络头文件------------end------------

//网络宏定义--------------start-------------
#define _PROT_ 9999
#define server_ip "192.168.43.169"  //Hi3861服务端的IP地址192.168.43.169
int sock_fd;
int addr_length;
//static const char *send_data = "Hello! I'm BearPi-HM_Nano TCP Client!\r\n";
char recvBuf[31];
char send_buf[50];
//网络宏定义--------------end-------------

#define GPIO7 7
#define Steer_Bottom_Io 6
#define Steer_Pitch_Io 7
#define Steer_High1_Io 8
#define Steer_High2_Io 11
#define Steer_Snatch_Io 12
#define Steer_Platform_Io 5

#define Left_infrared 13
#define Right_infrared 10

#define GPIO12 12

#define COUNT 3
#define TASK_STAK_SIZE    (1024*30)
#define UART_BUFF_SIZE 100
#define LED_TEST_GPIO 9 // for hispark_pegasus

#define WIFI_IOT_UART_IDX_1 1

unsigned int Steer_Bottom_duty = 1450;
unsigned int Steer_Pitch_duty = 1833;
unsigned int Steer_High1_duty = 1055;
unsigned int Steer_High2_duty = 2000;
unsigned int Steer_Snatch_duty = 1611;
unsigned int state = 0;

unsigned int flag = 0;
IotGpioValue Left_hinder;
IotGpioValue Right_hinder;


static const char *data = "next\r\n";

//舵机响应函数
void set_angle(unsigned int duty,unsigned int num)
{
    unsigned int time = 20000;
    unsigned int i = 0;
    IoTGpioSetDir(num, IOT_GPIO_DIR_OUT);
    // for(i = 0;i<COUNT;i++)
    // {
        IoTGpioSetOutputVal(num, IOT_GPIO_VALUE1);
        hi_udelay(duty);
        IoTGpioSetOutputVal(num, IOT_GPIO_VALUE0);
        hi_udelay(time - duty);
    // }
}


//减速函数
void slow_down(unsigned int current_duty, unsigned int expect_duty,unsigned int num)
{
    if(current_duty == expect_duty)
    {
        set_angle(expect_duty,num); 
    }

    if(current_duty > expect_duty)
    {
        while(current_duty > expect_duty)
        { 
            for (int i = 0; i < COUNT; i++) 
            {
                set_angle(current_duty,num);   
                current_duty -= 25;  
            }
        }
        if(current_duty != expect_duty)
        {
            current_duty = expect_duty;
            for (int i = 0; i < COUNT; i++) 
            {
                set_angle(expect_duty,num);   
            }
        }
    }

    if(current_duty < expect_duty)
    {
        while(current_duty < expect_duty)
        {
            for (int i = 0; i < COUNT; i++) 
            {
                set_angle(current_duty,num);
                current_duty += 25;
            }
        }
        if(current_duty != expect_duty)
        {
            current_duty = expect_duty;
            for (int i = 0; i < COUNT; i++) 
            {
                set_angle(current_duty,num);   
            }
        }
    }
}

//校正函数，精度1度
void check_left_right(int angel)
{
    unsigned char i = 0;
    IoTGpioGetInputVal(Left_infrared, &Left_hinder);
    IoTGpioGetInputVal(Right_infrared, &Right_hinder);

    for( i = 0; i<2 ;i++)
    {
        while(!(Left_hinder && Right_hinder))
        {
            printf("checking\n");
            printf("Left: %d\n",Left_hinder);
            printf("Right: %d\n",Right_hinder);
            if(Left_hinder)
            {
                angel -= 1;
                steer_bottom_set_angle(angel);
                hi_udelay(500000);
                printf("Left!  %d\n",angel);
            }

            if(Right_hinder)
            {
                angel += 1;
                steer_bottom_set_angle(angel);
                hi_udelay(500000);
                printf("Right!  %d\n",angel);            
            }
            printf("角度：%d\n",angel);
            IoTGpioGetInputVal(Left_infrared, &Left_hinder);
            hi_udelay(500000);
            IoTGpioGetInputVal(Right_infrared, &Right_hinder);
        }
        hi_udelay(2000000);
    }
    printf("结束校正\n");
}


void steer_bottom_set_angle(int angle)//底部舵机面向车前，左正右负
{
    unsigned int duty = 1450;
    duty += (angle*100/18);
    slow_down(Steer_Bottom_duty,duty,Steer_Bottom_Io);
    Steer_Bottom_duty = duty;
}




void steer_pitch_set_angle(int angle)//底部舵机面向车前，上正下负
{
    unsigned int duty = 1500;
    duty += (angle*100/9);
    slow_down(Steer_Pitch_duty,duty,Steer_Pitch_Io);
    Steer_Pitch_duty = duty;   
}

void steer_high1_set_angle(int angle)//第一台高度舵机面向车前，上正下负
{
    unsigned int duty = 1500;
    duty += (-angle*100/9);
    slow_down(Steer_High1_duty,duty,Steer_High1_Io);
    Steer_High1_duty = duty; 
}

void steer_high2_set_angle(int angle)//第二台高度舵机面向车前，上正下负
{
    unsigned int duty = 1500;
    duty += (angle*100/9);
    slow_down(Steer_High2_duty,duty,Steer_High2_Io);
    Steer_High2_duty = duty;   
}

void steer_snatch_set_angle(int angle)//夹取舵机，20是紧闭，80是完全松开,参数：松开为80度，闭合为20度
{
     unsigned int duty = 2500;
     duty += (-angle*100/9);
     slow_down(Steer_Snatch_duty,duty,Steer_Snatch_Io);
     Steer_Snatch_duty = duty;      
}

void steer_platform_set_angle(int angle)//摄像头舵机面向车前，上正下负，-40度到40度
{
    unsigned int duty = 1500;
    duty +=(angle*100/9);    
     for (int i = 0; i < COUNT; i++) {
        set_angle(duty,Steer_Platform_Io);
    }   
}

void sterr_init()
{
    // printf("The steering gear is init\r\n");

    steer_pitch_set_angle(-15);
    printf("pitch\n"); 

    steer_bottom_set_angle(0);
    printf("bottom\n");

    steer_high1_set_angle(45);  
    printf("high1\n");

    steer_high2_set_angle(75);
    printf("high2\n");

    hi_udelay(2000000);

    check_left_right(0);
    
    steer_high1_set_angle(65);  

    steer_snatch_set_angle(80);                                                                                                                                       
    printf("snatch\n");

//待命
    printf("运行一次");  
    // snatch_task();  
}

void snatch_task()
{
    steer_snatch_set_angle(10);                                                                                                                                       
    printf("snatch\n");

    steer_high2_set_angle(85);
    printf("high2\n");

    steer_high1_set_angle(90);  
    printf("high1\n");

    steer_pitch_set_angle(0);
    printf("pitch\n"); 

}

void transfer_task()
{
    steer_high2_set_angle(5);
    printf("high2\n");

    steer_high1_set_angle(70);
    printf("high1\n"); 

    steer_snatch_set_angle(60);                                                                                                                                       
    printf("snatch\n");

    steer_high2_set_angle(85);
    printf("high2\n");

    steer_high1_set_angle(45);  
    printf("high1\n");
    
    steer_pitch_set_angle(40);
    printf("pitch\n"); 

    steer_high2_set_angle(0);  
    printf("high1\n");    

    steer_bottom_set_angle(0);
    printf("bottom\n");    

}

void Infrared_init()
{
    IoSetFunc(IOT_IO_NAME_GPIO_13,IOT_IO_FUNC_GPIO_13_GPIO);
    IoTGpioSetDir(IOT_IO_NAME_GPIO_13,IOT_GPIO_DIR_IN);
    IoSetPull(IOT_IO_NAME_GPIO_13,IOT_IO_PULL_UP);   

    IoSetFunc(IOT_IO_NAME_GPIO_10,IOT_IO_FUNC_GPIO_10_GPIO);
    IoTGpioSetDir(IOT_IO_NAME_GPIO_10,IOT_GPIO_DIR_IN);
    IoSetPull(IOT_IO_NAME_GPIO_10,IOT_IO_PULL_UP);   
}
unsigned char check_msg(uint8_t *uart_buff_ptr,unsigned int length)
{
    uint8_t Scoket_send[30] = {0};
    unsigned char j = 0;
    unsigned char num = 0;
    unsigned char box_num = 0;
    num = (uart_buff_ptr[0]-'0')*10 + (uart_buff_ptr[1]- '0');
    if(num == (length-2))
    {
        num = 0;
        printf("Correct!\n");
        box_num = (uart_buff_ptr[3] - '0');
        for(j = 2; j<length; j++)
        {
            Scoket_send[num++] = uart_buff_ptr[j];
        }
        printf("Wifi send: %s\n",Scoket_send);
        printf("num = %d\n",box_num);
        //wifisend函数
        // IoTUartWrite(WIFI_IOT_UART_IDX_1, (unsigned char *)data, strlen(data));
        return box_num;
    }
    else
    {
        return 0;
         //box_num = (uart_buff_ptr[3] - '0');
         //return box_num;
    }

}

void check()
{
    while (1)
    {
        IoTGpioGetInputVal(Left_infrared, &Left_hinder);
        IoTGpioGetInputVal(Right_infrared, &Right_hinder);
        printf("Left:%d\n",Left_hinder);
        printf("Right:%d\n",Right_hinder);
        hi_udelay(500000);  
    }
}
//调试函数

/* 任务实现 */
void RobotTask(void* parame)
{
    (void)parame;
    uint8_t uart_buff[UART_BUFF_SIZE] = { 0 };
    uint8_t *uart_buff_ptr = uart_buff;
    unsigned int duty = 500;
    unsigned int recv_num = 0;
    unsigned int j = 0;
    unsigned char box_num = 0;


    //网络连接部分---------------------start----------------------------
    IoTWatchDogDisable();//禁止看门狗
    //服务器的地址信息
    struct sockaddr_in send_addr;
    socklen_t addr_length = sizeof(send_addr);
    
    //连接Wifi,局域网
    WifiConnect("Fle", "gxd001213");
    //创建socket
    if ((sock_fd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
    {
        perror("create socket failed!\r\n");
        exit(1);
    }

    //初始化预连接的服务端地址
    send_addr.sin_family = AF_INET;
    send_addr.sin_port = htons(_PROT_);
    send_addr.sin_addr.s_addr = inet_addr(server_ip);
    addr_length = sizeof(send_addr);
    while(1)
    {
        if (connect(sock_fd, (struct sockaddr *)&send_addr, addr_length) >= 0) //-1表示连接失败
        {
            printf("connect success\r\n");
            break;
        }
        else
        {
            printf("connecting\r\n");
        }
    }
    //网络连接部分---------------------end----------------------------
    // IoSetFunc(IOT_IO_NAME_GPIO_0,IOT_IO_FUNC_GPIO_0_UART1_TXD);//
    // IoSetFunc(IOT_IO_NAME_GPIO_1,IOT_IO_FUNC_GPIO_1_UART1_RXD);//
    // IotUartAttribute uart_attr = {

    //     // baud_rate: 9600
    //     .baudRate = 115200,

    //     // data_bits: 8bits
    //     .dataBits = 8,
    //     .stopBits = 1,
    //     .parity = 0,
    // };
    // // // // Initialize uart driver
    // IoTUartInit(WIFI_IOT_UART_IDX_1, &uart_attr);
    IoTWatchDogDisable();
    Infrared_init();      
    //不能动
    sterr_init();


    while(flag)
    {
        switch(state)
        {
            case 0:
                {
                    //recv_num = IoTUartRead(WIFI_IOT_UART_IDX_1, uart_buff_ptr, UART_BUFF_SIZE);
                    memset(recvBuf,0,sizeof(recvBuf));
                    printf("before recv recvBuf:%s\r\n",recvBuf);
                    while(!recv_num)
                    {
                        recv(sock_fd, recvBuf, sizeof(recvBuf), 0);  //阻塞
                        recv_num=strlen(recvBuf);
                    }
                    printf("recvBuf=%s\r\n",recvBuf);
                     if(recv_num)
                     {
                        printf("Recv\n");
                        printf("wifi read data:%d\n", recv_num);
                        printf("wifi read data:%s\n", recvBuf);
                        box_num = check_msg(recvBuf,recv_num);
                        sprintf(send_buf,"%d %s",box_num,recvBuf);
                        send(sock_fd, send_buf, strlen(send_buf), 0);
                        // check_msg(uart_buff_ptr,i); 
                        printf("num = %d\n",box_num); 
                        if((box_num<=4) && box_num)
                        {
                            state = 1;
                        }
                     }
                    hi_udelay(1000000);
                }
            break;
            //case1:等待接收上位机信息,鉴别并发送信息到PC端

            case 1: 
                    printf("运行一次!\n");
                    steer_platform_set_angle(0);
                    steer_platform_set_angle(-45);
                    hi_udelay(2000000);
                    steer_platform_set_angle(0);
                    snatch_task();
                    state = 2;
                    break;
            //校准，夹取书本

            case 2:
                    switch(box_num)
                    {
                        case 1: steer_bottom_set_angle(90);transfer_task();printf("1号箱子\n");break;
                        case 2: steer_bottom_set_angle(180);transfer_task();printf("2号箱子\n");break;
                        case 3: steer_bottom_set_angle(-90);transfer_task();printf("3号箱子\n");break;
                        case 4: steer_bottom_set_angle(-180);transfer_task();printf("4号箱子\n");break;
                        default:steer_bottom_set_angle(90);transfer_task();printf("1号箱子\n");break;
                    }
                    state = 0;
                    sterr_init();
                    printf("again!\n");
                    recv_num = 0;
                    break;

            //根据框号放置到书框，并返回状态1

        }
    }
}



static void RobotDemo(void)
{
    osThreadAttr_t attr;

    attr.name = "RobotTask";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = TASK_STAK_SIZE;
    attr.priority = osPriorityNormal;

    if (osThreadNew(RobotTask, NULL, &attr) == NULL) {
        printf("[RobotDemo] Falied to create RobotTask!\n");
        flag = 0;
    }
    {
        flag = 1;
    }
}

APP_FEATURE_INIT(RobotDemo);
